#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <libevdev-1.0/libevdev/libevdev.h>
#include <linux/input.h>
#include <fcntl.h>
#include <unistd.h>
#include <cmath>
#include <vector>
#include <dirent.h>
#include <string>
#include <cstring>

class GamepadController : public rclcpp::Node {
public:
    GamepadController() : Node("gamepad_controller") {
        declare_parameter("device", "");
        std::string device_path = get_parameter("device").as_string();

        if (device_path.empty()) {
            auto devices = find_gamepads();
            if (devices.empty()) {
                RCLCPP_FATAL(this->get_logger(), "没有找到游戏手柄！");
                exit(EXIT_FAILURE);
            }
            
            print_devices(devices);
            RCLCPP_FATAL(this->get_logger(), 
                       "请通过参数指定设备: ros2 run <包名> <节点名> --ros-args -p device:=/dev/input/eventX");
            exit(EXIT_FAILURE);
        }

        if (!init_device(device_path)) {
            RCLCPP_FATAL(this->get_logger(), "设备初始化失败!");
            exit(EXIT_FAILURE);
        }

        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50),
            std::bind(&GamepadController::update, this));
    }

private:
    struct GamepadInfo {
        std::string path;
        std::string name;
        int vendor_id;
        int product_id;
    };

    static const char* event_type_to_string(unsigned int type) {
        switch (type) {
            case EV_SYN:    return "SYN";
            case EV_KEY:    return "KEY";
            case EV_ABS:    return "ABS";
            case EV_REL:    return "REL";
            case EV_MSC:    return "MSC";
            case EV_SW:     return "SW";
            case EV_LED:    return "LED";
            case EV_SND:    return "SND";
            case EV_REP:    return "REP";
            case EV_FF:     return "FF";
            case EV_PWR:    return "PWR";
            case EV_FF_STATUS: return "FF_STATUS";
            default:        return "UNKNOWN";
        }
    }

    static const char* key_code_to_string(unsigned int code) {
        switch(code) {
            case BTN_SOUTH:     return "B";
            case BTN_EAST:      return "A";
            case BTN_NORTH:     return "X";
            case BTN_WEST:      return "Y";
            case BTN_TL:        return "LB";
            case BTN_TR:        return "RB";
            case BTN_TL2:       return "LT";
            case BTN_TR2:       return "RT";
            case BTN_SELECT:    return "SELECT";
            case BTN_START:     return "START";
            case BTN_MODE:      return "HOME";
            case BTN_THUMBL:    return "L3";
            case BTN_THUMBR:    return "R3";
            case BTN_DPAD_UP:   return "DPAD_UP";
            case BTN_DPAD_DOWN: return "DPAD_DOWN";
            case BTN_DPAD_LEFT: return "DPAD_LEFT";
            case BTN_DPAD_RIGHT:return "DPAD_RIGHT";
            default:            return "UNKNOWN_KEY";
        }
    }

    static const char* abs_code_to_string(unsigned int code) {
        switch(code) {
            case ABS_X:         return "LeftX";
            case ABS_Y:         return "LeftY";
            case ABS_RX:        return "RightX";
            case ABS_RY:        return "RightY";
            case ABS_Z:         return "LT";
            case ABS_RZ:        return "RT";
            case ABS_HAT0X:     return "DPadX";
            case ABS_HAT0Y:     return "DPadY";
            default:            return "UNKNOWN_AXIS";
        }
    }

    std::vector<GamepadInfo> find_gamepads() {
        std::vector<GamepadInfo> gamepads;
        DIR *dir = opendir("/dev/input");
        if (!dir) return gamepads;

        struct dirent *ent;
        while ((ent = readdir(dir)) != nullptr) {
            if (strncmp(ent->d_name, "event", 5) != 0) continue;

            std::string path = "/dev/input/" + std::string(ent->d_name);
            if (is_gamepad(path)) {
                gamepads.push_back(get_gamepad_info(path));
            }
        }
        closedir(dir);
        return gamepads;
    }

    GamepadInfo get_gamepad_info(const std::string& path) {
        GamepadInfo info;
        int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK);
        if (fd < 0) return info;

        struct libevdev *dev;
        if (libevdev_new_from_fd(fd, &dev) == 0) {
            info.path = path;
            info.name = libevdev_get_name(dev);
            info.vendor_id = libevdev_get_id_vendor(dev);
            info.product_id = libevdev_get_id_product(dev);
            libevdev_free(dev);
        }
        close(fd);
        return info;
    }

    bool is_gamepad(const std::string& path) {
        int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK);
        if (fd < 0) return false;

        struct libevdev *dev;
        bool valid = false;
        if (libevdev_new_from_fd(fd, &dev) == 0) {
            valid = libevdev_has_event_type(dev, EV_ABS) &&
                    libevdev_has_event_code(dev, EV_KEY, BTN_GAMEPAD);
            libevdev_free(dev);
        }
        close(fd);
        return valid;
    }

    void print_devices(const std::vector<GamepadInfo>& devices) {
        RCLCPP_INFO(this->get_logger(), "可用的游戏手柄设备:");
        for (const auto& dev : devices) {
            RCLCPP_INFO(this->get_logger(), "  %-18s %-20s (VID: 0x%04x PID: 0x%04x)",
                      dev.path.c_str(),
                      dev.name.c_str(),
                      dev.vendor_id,
                      dev.product_id);
        }
    }

    bool init_device(const std::string& path) {
        RCLCPP_INFO(this->get_logger(), "初始化设备中: %s", path.c_str());

        fd_ = open(path.c_str(), O_RDONLY | O_NONBLOCK);
        if (fd_ < 0) {
            RCLCPP_ERROR(this->get_logger(), "打开设备失败: %s (%s)",
                       path.c_str(), strerror(errno));
            return false;
        }

        if (libevdev_new_from_fd(fd_, &dev_) != 0) {
            RCLCPP_ERROR(this->get_logger(), "初始化libevdev失败");
            close(fd_);
            return false;
        }

        RCLCPP_INFO(this->get_logger(), "━━━━━━━━━━━━━━━━━━━━━━━");
        RCLCPP_INFO(this->get_logger(), " 设备名称: %s", libevdev_get_name(dev_));
        RCLCPP_INFO(this->get_logger(), " 物理地址: %s", libevdev_get_phys(dev_));
        RCLCPP_INFO(this->get_logger(), " 支持的事件类型:");
        
        for (int type = 0; type <= EV_MAX; type++) {
            if (libevdev_has_event_type(dev_, type)) {
                RCLCPP_INFO(this->get_logger(), "   %-12s (%d)", 
                          event_type_to_string(type), type);
            }
        }
        RCLCPP_INFO(this->get_logger(), "━━━━━━━━━━━━━━━━━━━━━━━");

        return true;
    }

    void update() {
        process_events();
        publish_twist();
    }

    void process_events() {
        struct input_event ev;
        while (libevdev_next_event(dev_, LIBEVDEV_READ_FLAG_NORMAL, &ev) == 0) {
            if (ev.type == EV_SYN) continue;

            switch (ev.type) {
                case EV_ABS:
                    handle_axis(ev.code, ev.value);
                    break;
                case EV_KEY:
                    handle_button(ev.code, ev.value);
                    break;
                default:
                    RCLCPP_DEBUG(this->get_logger(), "Unhandled event type: %s (%d)",
                               event_type_to_string(ev.type), ev.type);
            }
        }
    }

    void handle_axis(int code, int value) {
        const char* axis_name = abs_code_to_string(code);
        double scaled_value = 0.0;

        switch(code) {
            case ABS_X:  // 左摇杆X轴控制角速度
                scaled_value = scale_joystick(value, true);
                angular_z_ = scaled_value * 2.0;
                break;
            case ABS_RX: // 右摇杆X轴控制线速度Y方向
                scaled_value = scale_joystick(value, true);
                linear_y_ = scaled_value * 5.0;
                break;
            case ABS_RY: // 右摇杆Y轴控制线速度X方向
                scaled_value = scale_joystick(value, true);
                linear_x_ = scaled_value * 5.0;
                break;
            case ABS_Y:  // 左摇杆Y轴未使用
            case ABS_Z:  // LT
                scaled_value = scale_trigger(value);
                lt_ = scaled_value;
                break;
            case ABS_RZ: // RT
                scaled_value = scale_trigger(value);
                rt_ = scaled_value;
                break;
            case ABS_HAT0X: // D-Pad X
                dpad_x_ = value;
                break;
            case ABS_HAT0Y: // D-Pad Y
                dpad_y_ = value;
                break;
        }

        RCLCPP_INFO(this->get_logger(), "[AXIS] %-8s Raw: %-6d Scaled: %+.2f", 
                   axis_name, value, scaled_value);
    }

    void handle_button(int code, int value) {
        const char* btn_name = key_code_to_string(code);
        
        RCLCPP_INFO(this->get_logger(), "[BTN] %-8s State: %s", 
                   btn_name, (value ? "PRESSED" : "RELEASED"));

        if (code == BTN_SOUTH && value) {
            RCLCPP_INFO(this->get_logger(), "A Button Pressed!");
        }
    }

    double scale_joystick(int value, bool invert) {
        double scaled = value / 32767.0;
        if (invert) scaled *= -1;
        return std::clamp(scaled, -1.0, 1.0);
    }

    double scale_trigger(int value) {
        return std::clamp(value / 255.0, 0.0, 1.0);
    }

    void publish_twist() {
        auto twist = geometry_msgs::msg::Twist();
        twist.linear.x = linear_x_;   // 右摇杆Y轴
        twist.linear.y = linear_y_;   // 右摇杆X轴
        twist.angular.z = angular_z_; // 左摇杆X轴
        cmd_vel_pub_->publish(twist);

        RCLCPP_DEBUG(this->get_logger(), 
                   "[STATUS] Linear X: %.2f Y: %.2f Angular: %.2f",
                   linear_x_, linear_y_, angular_z_);
    }

    // ROS components
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::TimerBase::SharedPtr timer_;

    // Device state
    int fd_ = -1;
    libevdev* dev_ = nullptr;
    double linear_x_ = 0.0;  // 右摇杆Y轴
    double linear_y_ = 0.0;  // 右摇杆X轴
    double angular_z_ = 0.0; // 左摇杆X轴
    double lt_ = 0.0;
    double rt_ = 0.0;
    int dpad_x_ = 0;
    int dpad_y_ = 0;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GamepadController>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}